package org.noote.libs.sogle;

import android.util.FloatMath;

public class Quaternion {
	private static final float TO_HALF_RAD = 3.14159265f / 360.0f;
	
	public float x=0, y=0, z=0, w=1;
	
	public Quaternion( )
	{
	}

	public Quaternion( float fx, float fy, float fz, float fw )
	{
		x = fx;
		y = fy;
		z = fz;
		w = fw;
	}

	// This just took four floats initially to avoid dependence on the vector class
	// but I decided avoiding confusion with the value setting constructor was more important
	public Quaternion( float Angle, Vector3D Axis)
	{
		setAxis( Angle, Axis );
	}

	// No rotation
	public void reset( )
	{
		x = 0;
		y = 0;
		z = 0;
		w = 1;
	}

	public void set(float fx, float fy, float fz, float fw ) {
		x = fx;
		y = fy;
		z = fz;
		w = fw;
	}
	
	// Set Quat from axis-angle
	public void setAxis( float degrees, Vector3D axis)
	{
		float HalfAngle = degrees * TO_HALF_RAD; // Get half angle in radians from angle in degrees
		float sinA = FloatMath.sin( HalfAngle ) ;
		w = FloatMath.cos( HalfAngle );
		x = axis.x * sinA;
		y = axis.y * sinA;
		z = axis.z * sinA;
	}

	public static Quaternion invert(Quaternion a)
	{
		return new Quaternion( -a.x, -a.y, -a.z, a.w );
	}

	// Note that order matters with concatenating Quaternion rotations
	public static Quaternion mult(Quaternion a, Quaternion b)
	{
		Quaternion r = new Quaternion();

		r.w = a.w*b.w - a.x*b.x  -  a.y*b.y  -  a.z*b.z;
		r.x = a.w*b.x + a.x*b.w  +  a.y*b.z  -  a.z*b.y;
		r.y = a.w*b.y + a.y*b.w  +  a.z*b.x  -  a.x*b.z;
		r.z = a.w*b.z + a.z*b.w  +  a.x*b.y  -  a.y*b.x;

		return r;
	}
	
	// You could add an epsilon to this equality test if needed
	public boolean equal ( Quaternion b )
	{
		return (x == b.x && y == b.y && z == b.z && w == b.w);
	}

	public boolean isIdentity( )
	{
		return (x == 0.0f && y == 0.0f && z == 0.0f && w==1.0f);
	}

	// Can be used the determine Quaternion neighbourhood
	public static float dot( Quaternion a, Quaternion b )
	{
		return a.x * b.x + a.y * b.y + a.z * b.z + a.w * b.w;
	}

	// Scalar multiplication
	public static Quaternion mult( Quaternion a, float s )
	{
		return new Quaternion(a.x * s, a.y * s, a.z * s, a.w * s );
	}
	
	public static Vector3D mult( Quaternion a, Vector3D v )
	{
		float fLen = v.getLenght();
		Vector3D vn = Vector3D.normalize(v);
		Quaternion qv = new Quaternion(vn.x, vn.y, vn.z, 0);
		Quaternion qvinv = Quaternion.mult(qv, Quaternion.invert(a));
		Quaternion qres = Quaternion.mult(a, qvinv);
		return new Vector3D(qres.x*fLen, qres.y*fLen, qres.z*fLen);
	}


	// Addition
	public static Quaternion add ( Quaternion a, Quaternion b )
	{
		return new Quaternion( a.x + b.x, a.y + b.y, a.z + b.z, a.w + b.w );
	}

	// ------------------------------------
	// Simple Euler Angle to Quaternion conversion, this could be made faster
	// ------------------------------------
	public static Quaternion fromEuler( float rx, float ry, float rz )
	{
		Quaternion qx = new Quaternion(-rx, new Vector3D( 1, 0, 0 ) );
		Quaternion qy = new Quaternion(-ry, new Vector3D( 0, 1, 0 ) );
		Quaternion qz = new Quaternion(-rz, new Vector3D( 0, 0, 1 ) );
		qz = Quaternion.mult(qy, qz);
		return Quaternion.mult(qx, qz);
	}

	// ------------------------------------
	// Quaternions store scale as well as rotation, but usually we just want rotation, so we can normalize.
	// ------------------------------------
	public int normalize( )
	{
		float lengthSq = x * x + y * y + z * z + w * w;

		if (lengthSq == 0.0 ) return -1;
		if (lengthSq != 1.0 )
			{
			float scale = ( 1.0f / FloatMath.sqrt( lengthSq ) );
			x *= scale;
			y *= scale;
			z *= scale;
			w *= scale;
			return 1;
			}
		return 0;
	}

	// ------------------------------------
	// Creates a value for this Quaternion from spherical linear interpolation
	// t is the interpolation value from 0 to 1
	// ------------------------------------
	public static Quaternion slerp(Quaternion a, Quaternion b, float t)
	{
	  float w1, w2;

	  float cosTheta = Quaternion.dot(a, b);
	  float theta    = (float)Math.acos(cosTheta);
	  float sinTheta = FloatMath.sin(theta);

	  if( sinTheta > 0.001f )
	  {
		w1 = FloatMath.sin( (1.0f-t)*theta ) / sinTheta;
		w2 = FloatMath.sin( t*theta) / sinTheta;
	  } else {
		// CQuat a ~= CQuat b
		w1 = 1.0f - t;
		w2 = t;
	  }

	  return Quaternion.add(Quaternion.mult(a, w1), Quaternion.mult(b, w2));
	}

	// ------------------------------------
	// linearly interpolate each component, then normalize the Quaternion
	// Unlike spherical interpolation, this does not rotate at a constant velocity,
	// although that's not necessarily a bad thing
	// ------------------------------------
	public static Quaternion NLerp( Quaternion a, Quaternion b, float w2)
	{
		float w1 = 1.0f - w2;
		Quaternion r = Quaternion.add(Quaternion.mult(a, w1), Quaternion.mult(b, w2));

		r.normalize();
		return r;
	}

	// ------------------------------------
	// Set a 4x4 matrix with the rotation of this Quaternion
	// ------------------------------------
	public float[] toMatrix_4x4()
	{
		float mf[] = new float[16];
		
		float x2 = 2.0f * x,  y2 = 2.0f * y,  z2 = 2.0f * z;

		float xy = x2 * y,  xz = x2 * z;
		float yy = y2 * y,  yw = y2 * w;
		float zw = z2 * w,  zz = z2 * z;

		mf[ 0] = 1.0f - ( yy + zz );
		mf[ 1] = ( xy - zw );
		mf[ 2] = ( xz + yw );
		mf[ 3] = 0.0f;

		float xx = x2 * x,  xw = x2 * w,  yz = y2 * z;

		mf[ 4] = ( xy +  zw );
		mf[ 5] = 1.0f - ( xx + zz );
		mf[ 6] = ( yz - xw );
		mf[ 7] = 0.0f;

		mf[ 8] = ( xz - yw );
		mf[ 9] = ( yz + xw );
		mf[10] = 1.0f - ( xx + yy );  
		mf[11] = 0.0f;  

		mf[12] = 0.0f;  
		mf[13] = 0.0f;   
		mf[14] = 0.0f;   
		mf[15] = 1.0f;
		
		return mf;
	}
	public void toMatrix_4x4(float mf[])
	{
		float x2 = 2.0f * x,  y2 = 2.0f * y,  z2 = 2.0f * z;

		float xy = x2 * y,  xz = x2 * z;
		float yy = y2 * y,  yw = y2 * w;
		float zw = z2 * w,  zz = z2 * z;

		mf[ 0] = 1.0f - ( yy + zz );
		mf[ 1] = ( xy - zw );
		mf[ 2] = ( xz + yw );
		mf[ 3] = 0.0f;

		float xx = x2 * x,  xw = x2 * w,  yz = y2 * z;

		mf[ 4] = ( xy +  zw );
		mf[ 5] = 1.0f - ( xx + zz );
		mf[ 6] = ( yz - xw );
		mf[ 7] = 0.0f;

		mf[ 8] = ( xz - yw );
		mf[ 9] = ( yz + xw );
		mf[10] = 1.0f - ( xx + yy );  
		mf[11] = 0.0f;  

		mf[12] = 0.0f;  
		mf[13] = 0.0f;   
		mf[14] = 0.0f;   
		mf[15] = 1.0f;
	}
}
